﻿// system
#include <stdlib.h>
#include <math.h>
#include <stdio.h> //printf需要
// gtracker
#include "gtrack.h"
// event
#include "lane_manager.h"

#pragma region 内部函数
/**
*  @b Description
*  @n
*     计算两点A，B的单位法向量（向左）
*  @param[in] a
*		点A
*  @param[in] b
*       点B
*  @param[out] out
*       法向量
*  @retval
*      None
*/
void calc_normal_vector(const Point2D* a, const Point2D* b, Point2D* out)
{
    Point2D tangent = { b->x - a->x, b->y - a->y };
    out->x = -tangent.y;
    out->y = tangent.x;

    // 单位化
    float length = sqrtf(out->x * out->x + out->y * out->y);
    if (length > 1e-6)
    {
        out->x /= length;
        out->y /= length;
    }
}

/**
*  @b Description
*  @n
*     根据当前曲线生成相邻车道的曲线
*  @param[in] ref
*		参考曲线
*  @param[in] width
*       车道宽度
*  @param[in] new_curve
*       新曲线
*  @retval
*      None
*/
void generate_adjacent_curve(const BezierCurve* ref, float width, BezierCurve* new_curve)
{
    Point2D n0, n1, n2;
    // 计算各控制点法线方向
    calc_normal_vector(&ref->p0, &ref->p1, &n0);
    calc_normal_vector(&ref->p0, &ref->p2, &n1); // 近似处理
    calc_normal_vector(&ref->p1, &ref->p2, &n2);

    // 偏移生成新控制点
    new_curve->p0.x = ref->p0.x + n0.x * width;
    new_curve->p0.y = ref->p0.y + n0.y * width;
    new_curve->p1.x = ref->p1.x + n1.x * width;
    new_curve->p1.y = ref->p1.y + n1.y * width;
    new_curve->p2.x = ref->p2.x + n2.x * width;
    new_curve->p2.y = ref->p2.y + n2.y * width;
}

/**
*  @b Description
*  @n
*     生成车道线位置，保存至表中（查表法）
*  @param[in] road
*		一个指向道路拖拓扑结构的指针
*  @retval
*      None
*/
void add_lane_data(RoadTopology* road)
{
    int maxStep = (int)road->lanes[0].curve[road->curve_number - 1].p2.y;        //基线车道最远端 直接设置好?

    for (uint8_t i = 0; i <= road->count; i++)
    {
        road->lanes[i].y2x = (float*)gtrack_alloc(maxStep, sizeof(float));
        LaneStrip* lane = &road->lanes[i];
        float a, b, c, u;
        int curveId = 0;
        float t = 0;
        for (int dis = 0; dis < maxStep; dis++)
        {
            float y_temp = (float)dis;
            if (y_temp > lane->curve[curveId].p2.y)
            {
                curveId++;
                if (curveId >= road->curve_number)
                    break;
            }
            if (y_temp <= lane->curve[curveId].p2.y)
            {
                a = lane->curve[curveId].p0.y + lane->curve[curveId].p2.y - 2.0f * lane->curve[curveId].p1.y;
                if (fabsf(a - 0) < 1e-5)
                {
                    t = -(lane->curve[curveId].p0.y - y_temp) / (2.0f * (lane->curve[curveId].p1.y - lane->curve[curveId].p0.y));
                    u = (1 - t);
                    lane->y2x[dis] = u * u * lane->curve[curveId].p0.x + 2 * u * t * lane->curve[curveId].p1.x + t * t * lane->curve[curveId].p2.x;
                }
                else
                {
                    b = (lane->curve[curveId].p1.y - lane->curve[curveId].p0.y) * 2.0f;
                    c = lane->curve[curveId].p0.y - y_temp;
                    t = (-b + sqrtf(b * b - 4.0f * a * c)) / (2.0f * a);
                    if (t < 0 || t > 1)
                    {
                        t = (-b - sqrtf(b * b - 4.0f * a * c)) / (2.0f * a);
                    }

                    if (t >= 0 && t <= 1)
                    {
                        float u = (1 - t);
                        lane->y2x[dis] = u * u * lane->curve[curveId].p0.x + 2 * u * t * lane->curve[curveId].p1.x + t * t * lane->curve[curveId].p2.x;
                    }
                    else
                    {
                        printf("无解?可能在近处和最远处会出现,此时t: %f,dis:%d,road:%d\n", t, dis, i);
                    }
                }
            }
            else
            {
                printf("必不可能打印,打印就是错了,此时y_temp :%f,lane->curve[curveId].p2.y :%f,curveId:%d\n", y_temp, lane->curve[curveId].p2.y, curveId);
            }
        }
    }
}

/**
*  @b Description
*  @n
*      判断两点A，B是否为不同点（坐标不同）
*  @param[in] p0
*		一个指向点A的指针
*  @param[in] p1
*       一个指向点B的指针
*
*  @retval
*      不同点：1，相同点：0
*/
int ifDiffrantePoint(Point2D* p0, Point2D* p1)
{
    if (fabsf(p0->x - p1->x) < 1e-5 && fabsf(p0->y - p1->y) < 1e-5)
        return 0;
    return 1;
}

/**
*  @b Description
*  @n
*      计算两个贝塞尔曲线A, B的交点（近似为曲线端点处的切线的交点）
*  @param[in] p2_A
*		曲线A的终点
*  @param[in] T_A
*       曲线A在终点处的切线
*  @param[in] p0_B
*		曲线B的起点
*  @param[in] T_A
*       曲线B在起点处的切线
*  @param[out] intersection
*       交点坐标
*  @retval
*      1：存在交点，0：不存在交点
*/
int calculate_tangent_intersection(
    const Point2D* p2_A, const Point2D* T_A, const Point2D* p0_B, const Point2D* T_B, Point2D* intersection)
{
    //x0 + t * x0' = x1 + s * x1' 
    //y0 + t * y0' = y1 + s * y1'
    //构造矩阵方程：A * [t; s] = b
    //变成 计算 t和s
    float a11 = T_A->x, a12 = -T_B->x;
    float a21 = T_A->y, a22 = -T_B->y;
    float b1 = p0_B->x - p2_A->x;
    float b2 = p0_B->y - p2_A->y;

    // 计算行列式
    float det = a11 * a22 - a12 * a21;
    if (fabs(det) < 1e-6)  // 切线平行或重合
    {
        return 0;
    }

    float t = (a22 * b1 - a12 * b2) / det;
    float s = (a11 * b2 - a21 * b1) / det;

    intersection->x = p2_A->x + t * T_A->x;
    intersection->y = p2_A->y + t * T_A->y;
    return 1;
}

/**
*  @b Description
*  @n
*      根据两个贝塞尔曲线的交点，调整贝塞尔曲线的参数
*  @param[out] p2_A
*		曲线A的最后两个控制点之一
*  @param[out] p1_A
*       曲线A的最后两个控制点之二
*  @param[out] p0_B
*		曲线B的前两个控制点之一
*  @param[out] p1_B
*       曲线B的前两个控制点之二
*  @param[in] Pc
*       交点坐标
*  @retval
*       None
*/
void adjust_curves_to_intersection(Point2D* p2_A, Point2D* p1_A, Point2D* p0_B, Point2D* p1_B, const Point2D* Pc)
{
    // 调整曲线A的终点和相邻控制点
    Point2D delta_A = { Pc->x - p2_A->x, Pc->y - p2_A->y };
    p2_A->x = Pc->x; // 更新终点为Pc
    p2_A->y = Pc->y;
    p1_A->x += delta_A.x * 0.5f; // 保持形状：调整前一个控制点
    p1_A->y += delta_A.y * 0.5f;

    // 调整曲线B的起点和相邻控制点
    Point2D delta_B = { Pc->x - p0_B->x, Pc->y - p0_B->y };
    p0_B->x = Pc->x; // 更新起点为Pc
    p0_B->y = Pc->y;
    p1_B->x += delta_B.x * 0.5f; // 保持形状：调整后一个控制点
    p1_B->y += delta_B.y * 0.5f;
}

/**
*  @b Description
*  @n
*      修复同一车道不同贝塞尔曲线的相交问题
*  @param[in] road
*		一个指向道路拖拓扑结构的指针
*  @retval
*      None
*/
void fix_joint_discontinuity(RoadTopology* road)
{
    for (int i = 0; i <= road->count; i++)
    {
        for (int t = 1; t < road->curve_number; t++)
        {
            if (ifDiffrantePoint(&road->lanes[i].curve[t].p0, &road->lanes[i].curve[t - 1].p2))
            {
                Point2D result;
                Point2D a;
                a.x = road->lanes[i].curve[t - 1].p2.x - road->lanes[i].curve[t - 1].p1.x;
                a.y = road->lanes[i].curve[t - 1].p2.y - road->lanes[i].curve[t - 1].p1.y;

                Point2D b;
                b.x = road->lanes[i].curve[t].p1.x - road->lanes[i].curve[t].p0.x;
                b.y = road->lanes[i].curve[t].p1.y - road->lanes[i].curve[t].p0.y;
                if (calculate_tangent_intersection(&road->lanes[i].curve[t - 1].p2, &a, &road->lanes[i].curve[t].p0, &b, &result))
                {
                    //方法1:
                    //adjust_curves_to_intersection(&road->lanes[i].curve[t - 1].p2, &road->lanes[i].curve[t - 1].p1, &road->lanes[i].curve[t].p0, &road->lanes[i].curve[t].p1, &result);
                    //方法2:
                    road->lanes[i].curve[t].p0 = result;
                    road->lanes[i].curve[t - 1].p2 = result;
                    //todo: 和不修改端点比差别,仿佛方法2更小,但是怎么证明合理
                }
            }
            //else
            //{
            //    if (i != 0)
            //        printf("第%d个已经修过了,边生成边改出现,否则不应该打印\n", i);
            //}
        }
    }
}

/**
*  @b Description
*  @n
*      计算贝塞尔曲线上点的坐标（贝塞尔曲线上的点的坐标由参数t确定）
*  @param[in] curve
*		一个指向贝塞尔曲线的指针
*  @param[in] t
*       贝塞尔曲线的参数t
*
*  @retval
*      参数t下点的坐标
*/
Point2D bezier_calculate(const BezierCurve* curve, float t)
{
    float u = 1.0f - t;
    float x_ = u * u * curve->p0.x + 2 * u * t * curve->p1.x + t * t * curve->p2.x;
    float y_ = u * u * curve->p0.y + 2 * u * t * curve->p1.y + t * t * curve->p2.y;
    Point2D Position = { x_,y_ };
    return Position;
}

/**
*  @b Description
*  @n
*      道路信息初始化
*  @param[in] road
*		一个指向道路拓扑结构的指针
*  @param[in] baseCurve
*       一个指向贝塞尔曲线的指针
*  @param[in] curve_number
*       贝塞尔曲线的个数
*  @param[in] width
*       道路宽度
*  @param[in] roadNums
*       车道个数
*  @retval
*      None
*/
void road_init(RoadTopology* road, const BezierCurve* baseCurve, uint8_t curve_number, float width, uint8_t roadNums)
{
    road->curve_number = curve_number;//有没有必要去根据实际输入扩展呢 这里记得后面修改，以达到和输入相关
    road->capacity = roadNums + 1; //车道数➕1个线
    road->count = 0;
    road->lanes = (LaneStrip*)gtrack_alloc( road->capacity, sizeof(LaneStrip));
    for (int t = 0; t < road->capacity; t++)
    {
        road->lanes[t].curve = (BezierCurve*)gtrack_alloc(curve_number, sizeof(BezierCurve));
    }

    if (baseCurve != NULL && road->lanes != NULL)
    {
        for(int i = 0 ;i < road->curve_number;i++)
            road->lanes[0].curve[i] = baseCurve[i];
        road->lanes[0].width = width;
        road->lanes[0].left_id = -1;
        road->lanes[0].right_id = -1;
    }
    // 初始化端点索引 - 暂时没用
    road->leftmost_id = 0;
    road->rightmost_id = 0;
}
#pragma endregion

void road_add_lane(RoadTopology* road, uint8_t roadNums, const BezierCurve* baseCurve, uint8_t curve_number, float width, InsertDirection insertDirection)
{
    if (roadNums < 1 )
    {
        return;
    }
    road_init(road, baseCurve, curve_number, width, roadNums);//生成基准车道线
    //循环添加有限数量个道路线
    for(; road->count < roadNums;)
    {
        uint8_t ref_lane_id = road->count ;
        // 生成新车道曲线
        LaneStrip* ref = &road->lanes[ref_lane_id];
        uint8_t cur_lane_id = ref_lane_id + 1;
        LaneStrip* new_lane = &road->lanes[cur_lane_id];
        road->count++;
        new_lane->width = ref->width;
        if (insertDirection == INSERT_RIGHT)
        {
            for(int i = 0;i< road->curve_number;i++)
                generate_adjacent_curve(&ref->curve[i], -ref->width, &new_lane->curve[i]); //向量是朝左的， width取负的
            //fix_joint_discontinuity(road); //效果一样
            new_lane->right_id = -1;
            new_lane->left_id = ref_lane_id;
            ref->right_id = road->count;
        }
        else if(insertDirection == INSERT_LEFT)
        {
            for (int i = 0; i < road->curve_number; i++)
                generate_adjacent_curve(&ref->curve[i], ref->width, &new_lane->curve[i]);
            //fix_joint_discontinuity(road);
            new_lane->left_id = -1;
            new_lane->right_id = ref_lane_id;
            ref->left_id = road->count;
        } 
        else
        {
            return;
        }
    }

    road->insertDirection = insertDirection;
    fix_joint_discontinuity(road);
    add_lane_data(road);
}

void road_adjust_width(RoadTopology* road, uint8_t lane_id, float new_width)
{
    if (lane_id > road->count || lane_id < 1) return;

    LaneStrip* lane = &road->lanes[lane_id -1];
    float delta = new_width - lane->width;
    lane->width = new_width;//修改的是索引为lane_id -1 的线

    // 保持其他车道宽度不变，递归更新左/右侧相邻车道
    switch (road->insertDirection)
    {
        case INSERT_LEFT:
            while (lane->left_id != -1)
            {
                LaneStrip* left = &road->lanes[lane->left_id];
                for(int i = 0 ; i< road->curve_number;i++)
                    generate_adjacent_curve(&left->curve[i], delta, &left->curve[i]);
                lane = left; //递归
            }
            break;
        case INSERT_RIGHT:
            while (lane->right_id != -1)
            {
                LaneStrip* right = &road->lanes[lane->right_id];
                for (int i = 0; i < road->curve_number; i++)
                    generate_adjacent_curve(&right->curve[i], -delta, &right->curve[i]);
                lane = right; //递归
            }
            break;
        default:
            break;
    }
    //要同步更改起止点及查表
    fix_joint_discontinuity(road);
    add_lane_data(road);
}

int8_t get_leftmost_lane(const RoadTopology* road)
{
    return road->leftmost_id;
}

int8_t get_rightmost_lane(const RoadTopology* road)
{
    return road->rightmost_id;
}

//void road_insert_lane(RoadTopology* road, uint8_t ref_lane_id, bool insert_to_right, float new_width)
//{
//    // 参数校验
//    if (ref_lane_id >= road->count) return;
//
//    // 扩展存储容量（每次翻倍）
//    if (road->count >= road->capacity) 
//    {
//        road->capacity = (road->capacity == 0) ? 4 : road->capacity * 2;
//        road->lanes = (LaneStrip*)realloc(road->lanes, sizeof(LaneStrip) * road->capacity);
//    }
//
//    // 获取参考车道和相邻车道
//    LaneStrip* ref_lane = &road->lanes[ref_lane_id];
//    int8_t neighbor_id = insert_to_right ? ref_lane->right_id : ref_lane->left_id;
//
//    // 生成新车道曲线
//    BezierCurve new_curve;
//    float offset = insert_to_right ? ref_lane->width : -new_width;
//    generate_adjacent_curve(&ref_lane->curve, offset, &new_curve);
//
//    // 初始化新车道
//    LaneStrip* new_lane = &road->lanes[road->count];
//    new_lane->curve = new_curve;
//    new_lane->width = new_width;
//    new_lane->left_id = insert_to_right ? ref_lane_id : neighbor_id;
//    new_lane->right_id = insert_to_right ? neighbor_id : ref_lane_id;
//    
//
//    // 更新参考车道关系
//    if (insert_to_right) 
//    {
//        ref_lane->right_id = road->count;
//    }
//    else 
//    {
//        ref_lane->left_id = road->count;
//    }
//
//    // 更新原相邻车道关系
//    if (neighbor_id != -1) 
//    {
//        LaneStrip* neighbor = &road->lanes[neighbor_id];
//        if (insert_to_right) 
//        {
//            neighbor->left_id = road->count;
//        }
//        else 
//        {
//            neighbor->right_id = road->count;
//        }
//    }
//
//    // 维护端点索引（新增核心逻辑）
//    if (insert_to_right) 
//    {
//        // 在参考车道右侧插入，且参考车道是当前最右侧
//        if (ref_lane_id == road->rightmost_id)
//        {
//            road->rightmost_id = road->count;
//        }
//    }
//    else 
//    {
//        // 在参考车道左侧插入，且参考车道是当前最左侧
//        if (ref_lane_id == road->leftmost_id) 
//        {
//            road->leftmost_id = road->count;
//        }
//    }
//
//    road->count++;
//}

float bezier_curvature(Point2D p0, Point2D p1, Point2D p2, float t) 
{
    // 参数有效性检查
    if (t < 0.0f || t > 1.0f) {
        return -1.0f;
    }

    // 计算一阶导数 (B'(t))
    float dx = 2.0f * ((1.0f - t) * (p1.x - p0.x) + t * (p2.x - p1.x));
    float dy = 2.0f * ((1.0f - t) * (p1.y - p0.y) + t * (p2.y - p1.y));

    // 计算二阶导数 (B''(t) 是常数)
    float ddx = 2.0f * (p2.x - 2.0f * p1.x + p0.x);
    float ddy = 2.0f * (p2.y - 2.0f * p1.y + p0.y);

    // 计算分子 |dx*ddy - ddx*dy|
    float numerator = fabsf(dx * ddy - ddx * dy);

    // 计算分母 (dx² + dy²)^(3/2)
    float denominator = powf(dx * dx + dy * dy, 1.5f);

    // 防止除零错误
    if (denominator < 1e-10) {
        return -1.0f;
    }

    return numerator / denominator;
}